#include "myInit.h"

static inline void myDisableEXTI_NVIC(uint8_t sNum)     //关闭某个外部中断
{
    EXTI->IMR1 &= ~(1 << sNum);        
}

float gShootLastAngle		= 0.f;
float gShootReadAngle       = 0.f;
float gShootNowAngle        = 0.f;
float gShootNowAngle_buff   = 0.f;


#if ANGLE_SOURCE == ANGLE_SOURCE_AS5047P
    AS5047P_HandleTypeDef shootEnc;
#endif


WS2811_HandleTypeDef LEDStrip;

#if ENC_SPEED_ENABLE
volatile float gShootSpeed 	= 0;
#endif

VESC_HandleTypeDef WheelLB;     //轮电机
VESC_HandleTypeDef WheelRB;
VESC_HandleTypeDef WheelRF;
VESC_HandleTypeDef WheelLF;
DJI_HandleTypeDef HelmLB;        //舵电机
DJI_HandleTypeDef HelmRB;
DJI_HandleTypeDef HelmRF;
DJI_HandleTypeDef HelmLF;

VESC_HandleTypeDef Shoot;     //推射电机

DJI_HandleTypeDef Defense;
DJI_HandleTypeDef NetL;
DJI_HandleTypeDef NetR;

volatile struct myErrState gErrState 			= {0};
volatile bool gMotorRxInitFinishFlag 			= false;
volatile bool gMechInitFinishAndStartEncFlag 	= false;
volatile bool gMechInitFinishAndInFreeRTOSFlag	= false;
volatile bool gMotorONFlag 						= false; 
volatile uint8_t gMechInitMode 					= false; 

uint16_t IDArray[CANNum] = {ShootID,
						#if ANGLE_SOURCE == ANGLE_SOURCE_CAN
							AS5047P_ID,
						#endif
                            WheelRFID, WheelRBID, WheelLBID, WheelLFID,
                            HelmLFID, HelmRFID, HelmRBID, HelmLBID,};

uint8_t IDFlagArray[CANNum] = {0};

void deviceInit()       //电子元等设备初始化
{
	communicateInit();
    WS2811_Init(LEDStrip,htim3,TIM_CHANNEL_1,32);
    LEDStrip.pf->setColor(&LEDStrip, 1, 32, 0xFF, 0xFF, 0xFF); // 白色, 代表上电
	// 定义CAN发送失败标志位
    
    VESC_Init(&Shoot, &hfdcan2, ShootID, 14, 1);            // 发射本杰明电机初始化

    VESC_Init(&WheelRF, &hfdcan1, WheelRFID, 7, 1);           // 轮电机初始化
	VESC_Init(&WheelRB, &hfdcan1, WheelRBID, 7, 1);
    VESC_Init(&WheelLF, &hfdcan1, WheelLFID, 7, 1);
    VESC_Init(&WheelLB, &hfdcan1, WheelLBID, 7, 1);
    DJI_Init(&HelmLF, &hfdcan1, HelmLFID);   // 舵电机初始化
	DJI_Init(&HelmRB, &hfdcan1, HelmRBID);
    DJI_Init(&HelmRF, &hfdcan1, HelmRFID);
    DJI_Init(&HelmLB, &hfdcan1, HelmLBID);

    DJI_Init(&NetL,&hfdcan3,NetLID);              // 运球电机初始化
    DJI_Init(&NetR,&hfdcan3,NetRID);
    DJI_Init(&Defense, &hfdcan3, DefenseID);

    // 各个电机CAN接收初始化
    VESC_RxInit(VESCNum, (VESC_HandleTypeDef*[]){&Shoot, &WheelLF, &WheelRF, &WheelLB, &WheelRB});
    DJI_RxInit(DJINum, (DJI_HandleTypeDef*[]){&NetL, &NetR});
	gMotorRxInitFinishFlag = true;
    
#if ANGLE_SOURCE == ANGLE_SOURCE_AS5047P
    AS5047P_Init(&shootEnc, &hspi1, GPIOA, GPIO_PIN_4);    	//推射编码器初始化
#endif
	
	HelmLB.pf->clearCount(&HelmLB);
	HAL_Delay(1);
	HelmLF.pf->clearCount(&HelmLF);
	HAL_Delay(1);
	HelmRB.pf->clearCount(&HelmRB);
	HAL_Delay(1);
	HelmRF.pf->clearCount(&HelmRF);
	HAL_Delay(1);
	
    HAL_TIM_Base_Start_IT(&htim5);  //启动定时器
}

void mechInit()     //机构初始化(复位等)
{
    volatile uint32_t sStartTick;
    sStartTick = HAL_GetTick();

#if SHOOT_ENABLE

    while (1)                    // 等待推射触发限位开关
    {
        if (HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_4) == GPIO_PIN_SET)
        {
            break;
        }
        Shoot.pf->dutyMode(&Shoot,-5.f);         //推射回到最底下(多次发防止timeOut)
        HAL_Delay(1);
    }
	while(!shootEnc.pf->read(&shootEnc))
	{
		HAL_Delay(1);
	}
	
    HAL_Delay(100);
	
	gShootLastAngle = shootEnc.angle;
	gMechInitFinishAndStartEncFlag = true;

    while(1)
    {
        if ((gShootNowAngle - cCatchAngle) > cDeathZone)
        {
            Shoot.pf->dutyMode(&Shoot, -5.f);
        }
        else if ((gShootNowAngle - cCatchAngle) < (-cDeathZone))
        {
            Shoot.pf->dutyMode(&Shoot, 10.f);
        }
        else
        {
            Shoot.pf->dutyMode(&Shoot, 0);
            break;
        }
        HAL_Delay(1);
    }
    HAL_Delay(1);
	
#endif

}

